
#include "quasinewton_matrix.h"

#include <gsl/gsl_blas.h>

void bfgs_hess_update(const gsl_vector *p, const gsl_vector *q, 
                      gsl_vector *Bp, gsl_matrix *B)
{
  double qp;
  double pBp;
  
  /* Compute auxiliary dot- and matrix-vector -products. */
  gsl_blas_ddot(q, p, &qp);
  gsl_blas_dsymv(CblasLower, 1.0, B, p, 0.0, Bp);
  gsl_blas_ddot(p, Bp, &pBp);
  
  /* Add the two correction terms. */
  gsl_blas_dsyr(CblasLower, 1.0 / qp, q, B);
  gsl_blas_dsyr(CblasLower, -1.0 / pBp, Bp, B);
}

void bfgs_invhess_update(const gsl_vector *p, const gsl_vector *q,
                         gsl_vector *Sq, gsl_matrix *S)
{
  double pq;
  double qSq;
  double m;
  
  gsl_blas_dcopy(q, Sq);
  gsl_blas_ddot(p, q, &pq);
  
  gsl_blas_dsymv(CblasLower, 1.0, S, q, 0.0, Sq);
  gsl_blas_ddot(q, Sq, &qSq);
  m = 1.0 + qSq / pq;
  
  /* S = S + first correction term. */
  gsl_blas_dsyr(CblasLower, m / pq, p, S);
  /* S = S - second correction term. */
  gsl_blas_dsyr2(CblasLower, -1.0 / pq, p, Sq, S);
}

void lbfgs_invhess_update(const gsl_vector *p, const gsl_vector *q,
                          const gsl_matrix *P, const gsl_matrix *Q,
                          int k, gsl_vector *alpha, gsl_vector *r)
{
  int i;
  int m;
  double rho;
  double pr;
  double qr;
  gsl_vector_view p_i, q_i;
  double alpha_i;
  double beta;
  int ri;
  
  /*if(P->size1 != Q->size1 TODO: error handling */
  
  m = P->size1;
  
  for(i = 0; i < m; i++)
  {
    ri = (k + i) % m;
    p_i = gsl_matrix_row(P, ri);
    q_i = gsl_matrix_row(Q, ri);
    
    gsl_blas_ddot(&p_i.vector, &q_i.vector, &rho);
    gsl_blas_ddot(&p_i.vector, r, &pr);
    alpha_i = rho * pr;
    gsl_vector_set(alpha, i, alpha_i);
    gsl_blas_daxpy(-alpha_i, &q_i.vector, r);
  }
  
  for(i = m - 1; i >= 0; i--)
  {
    ri = (k + i) % m;
    p_i = gsl_matrix_row(P, ri);
    q_i = gsl_matrix_row(Q, ri);
    
    gsl_blas_ddot(&p_i.vector, &q_i.vector, &rho);
    gsl_blas_ddot(&q_i.vector, r, &qr);
    beta = rho * qr;
    gsl_blas_daxpy(gsl_vector_get(alpha, i) - beta, &p_i.vector, r);
  }
}

void sr1_hess_update(const gsl_vector *p, const gsl_vector *q, 
                     gsl_vector *r, gsl_matrix *B)
{
  double d;
  
  r = gsl_vector_alloc(p->size);
  
  gsl_blas_dcopy(q, r);
  gsl_blas_dgemv(CblasNoTrans, -1.0, B, p, 1.0, r);
  gsl_blas_ddot(r, p, &d);
  
  gsl_blas_dsyr(CblasLower, 1.0 / d, r, B);
  
  gsl_vector_free(r);
}
